# -*- coding: UTF-8 -*-
#! /usr/bin/env python
import rospy
import actionlib
from camaction.msg import *
from Class.Camera import *
import math

class MyActionServer:
    def __init__(self):
        #SimpleActionServer(name, ActionSpec, execute_cb=None, auto_start=True)
        self.server = actionlib.SimpleActionServer("cam_serve",CamAction,self.cb,False)
        self.server.start()
        self.cam = Camera()
        rospy.loginfo("服务端启动")

    def cb(self,goal):

        rospy.loginfo("服务端处理请求:")
        
        u = goal.u
        v = goal.v
        get_depth = goal.get_depth

        result = CamResult()
        result.image = self.cam.getimageros()
        # print(type(self.cam.getimageros()))
        if get_depth:
            dis = self.cam.getdistance([int(u),int(v)])
            if math.isnan(dis):
                dis = 0
            result.depth = dis

        self.server.set_succeeded(result)
        rospy.loginfo("响应:%s",str(result.depth))
        
if __name__ == "__main__":
    rospy.init_node("action_cam_serve")
    server = MyActionServer()
    rospy.spin()
